#pragma once
#include <math.h>
#include <stdint.h>

#define EIGEN_NO_DEBUG 0
// #define EIGEN_DEFAULT_TO_ROW_MAJOR 1 // 在包含 Eigen 头文件前定义！
#include ".\Eigen\Core"
#include ".\Eigen\Geometry"

#define USE_LLH_Projection          0   // 0：使用经纬度；1：使用方位等距投影
#define NSAMPLES                    2
#define IMU_FRQ                     403                                   // Hz
#define AVPT_RING_BUFFER_LENGTH     (int)ceil(IMU_FRQ / NSAMPLES / 3)     // ~330ms

// C++ Class
typedef Eigen::Vector3d CVec3;
typedef Eigen::Matrix3d CMat3;
typedef Eigen::Matrix<double, 15, 1 > CVec15;
typedef Eigen::Matrix<double, 15, 15> CMat15;
typedef Eigen::Quaterniond CQuat;
extern  CVec3 O31; 

// C
typedef double Vec3[3];
// typedef double Mat3[9];
// typedef double Quat[4];
typedef double Vec15[15];
// typedef double Mat15[225];

#pragma pack(push, 4)

enum { E = 0, N, U };
enum { X = 0, Y, Z };
enum { i = 0, j, k };

enum NAV_PROCESS_ENUM
{ 
    Init_Alignment,
    INav_GNSSFuse,
    Dead_Reckoning,
    ZARUPT          // ZUPT 
};
extern enum NAV_PROCESS_ENUM NAV_PROCESS;

typedef struct
{
    double Re;
    double G0, r2d, d2r;
    double ug, ugpshz, ugpsh;
    double dps, dph, dpsh, dphpsh;
} glv_s;
extern const glv_s glv;

typedef struct
{
    double gyrBiasRepeatability;   // deg/h
    Vec3   accBias;                // ug
    Vec3   ARW, VRW;               // for Qt
    double sqrtR0G, tauG;          // bias Markov process
    double sqrtR0A, tauA;
} imuParam_s;
extern const imuParam_s imuParam;

typedef struct
{
    Vec3 um982_lever;
    struct
    {
        double yaw;
        Vec3 spp_vel, spp_pos;
        Vec3 rtk_vel, rtk_pos;
    } um982_noise;
    
    double baroHgt_noise;
    double magYaw_noise;
} obsParam_s;
extern const obsParam_s obsParam;
#pragma pack(pop)

namespace math
{
    extern const double PI;
    extern const double MAX_LAT;
    extern const double MAX_LON;
    extern const double MAX_HGT;
    
    template <typename T>
    constexpr T min(T val1, T val2)
    {
        return (val1 < val2) ? val1 : val2;
    }

    template <typename T>
    constexpr T max(T val1, T val2)
    {
        return (val1 > val2) ? val1 : val2;
    }

    template <typename T>
    constexpr T constrain(T val, T min, T max)
    {
        return (val < min) ? min : ((val > max) ? max : val);
    }

    template <typename T>
    constexpr T sq(T val)
    {
        return (val * val);
    }

    CVec3 q2rv(const CQuat &q);
    CQuat rv2q(const CVec3 &v);
    CVec3 q2att(const CQuat &qnb);
    CVec3 m2att(const CMat3 &Cnb);
    CMat3 q2mat(const CQuat &qnb);
    CMat3 a2mat(const CVec3 &att);
    CQuat a2qua(const CVec3 &att);
    CQuat m2qua(const CMat3 &Cnb);
    CVec3 qq2phi(const CQuat &qCalc, const CQuat &qReal);
    CQuat qdelphi(const CQuat &q, const CVec3 &phi);
}
